{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<center><img src=\"../image/CLBLOGO.jpg\" alt=\"创乐博\" style=\"width: 300px;\"/></center>\n",
    "\n",
    "# 10.OpenCV 摄像头云台物体追踪\n",
    "\n",
    "@－－－－湖南创乐博智能科技有限公司－－－－<br>\n",
    "@  文件名：10.OpenCV摄像头云台物体追踪.ipynb <br>\n",
    "@  版本：V2.0 <br>\n",
    "@  author: zhulin<br>\n",
    "@  说明：OpenCV摄像头云台物体追踪<br>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "tags": []
   },
   "source": [
    "## 为了可以打开摄像头，这里关闭摄像头所有进程"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 载入必要的库\n",
    "import cv2\n",
    "import numpy as np\n",
    "from LOBOROBOT import LOBOROBOT # 载入机器人库\n",
    "import time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "clbrobot = LOBOROBOT()  # 实例化机器人对象\n",
    "# Configure min and max servo pulse lengths\n",
    "servo_min = 150  # Min pulse length out of 4096\n",
    "servo_max = 600  # Max pulse length out of 4096"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# 舵机调零\n",
    "pan =  90\n",
    "tilt = -10\n",
    "# 频率设置为50hz，适用于舵机系统。\n",
    "clbrobot.set_servo_angle(10,pan)  # 底座舵机 90 \n",
    "clbrobot.set_servo_angle(9,tilt)  # 顶部舵机 30\n",
    "time.sleep(0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import traitlets\n",
    "import ipywidgets.widgets as widgets\n",
    "from IPython.display import display\n",
    "\n",
    "def bgr8_to_jpeg(value, quality=75):\n",
    "    return bytes(cv2.imencode('.jpg', value)[1])\n",
    "\n",
    "FGmaskComp_img = widgets.Image(format='jpeg', width=320, height=240)\n",
    "frame_img = widgets.Image(format='jpeg', width=320, height=240)\n",
    "\n",
    "dispaly_img = widgets.HBox([FGmaskComp_img,frame_img])\n",
    "display(dispaly_img)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 线程函数操作库\n",
    "import threading # 线程\n",
    "import ctypes\n",
    "import inspect\n",
    "\n",
    "# 线程结束代码\n",
    "def _async_raise(tid, exctype):\n",
    "    tid = ctypes.c_long(tid)\n",
    "    if not inspect.isclass(exctype):\n",
    "        exctype = type(exctype)\n",
    "    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))\n",
    "    if res == 0:\n",
    "        raise ValueError(\"invalid thread id\")\n",
    "    elif res != 1:\n",
    "        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)\n",
    "        raise SystemError(\"PyThreadState_SetAsyncExc failed\")\n",
    "        \n",
    "def stop_thread(thread):\n",
    "    _async_raise(thread.ident, SystemExit)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import libcamera\n",
    "from picamera2 import Picamera2\n",
    "\n",
    "picamera = Picamera2()\n",
    "config = picamera.create_preview_configuration(main={\"format\": 'RGB888', \"size\": (320, 240)},\n",
    "                                               raw={\"format\": \"SRGGB12\", \"size\": (1920, 1080)})\n",
    "config[\"transform\"] = libcamera.Transform(hflip=0, vflip=0)\n",
    "picamera.configure(config)\n",
    "picamera.start()\n",
    "\n",
    "dispW=320\n",
    "dispH=240\n",
    "\n",
    "width=320\n",
    "height=240"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "hueLower = widgets.IntSlider(min=96,max=179,step=1,description='hueLower：',value=96)\n",
    "hueUpper = widgets.IntSlider(min=120,max=179,step=1,description='hueUpper：',value=120)\n",
    "\n",
    "hue2Lower = widgets.IntSlider(min=50,max=179,step=1,description='hue2Lower：',value=50)\n",
    "hue2Upper = widgets.IntSlider(min=0,max=179,step=1,description='hue2Upper：',value=0)\n",
    "\n",
    "satLow = widgets.IntSlider(min=157,max=255,step=1,description='satLow：',value=157)\n",
    "satHigh = widgets.IntSlider(min=255,max=255,step=1,description='satHigh：',value=255)\n",
    "\n",
    "valLow = widgets.IntSlider(min=100,max=255,step=1,description='valLow：',value=100)\n",
    "valHigh = widgets.IntSlider(min=255,max=255,step=1,description='valHigh：',value=255)\n",
    "\n",
    "slider_img=widgets.VBox([\n",
    "              widgets.HBox([hueLower,hueUpper]),\n",
    "              widgets.HBox([hue2Lower,hue2Upper]),\n",
    "              widgets.HBox([satLow,satHigh]),\n",
    "              widgets.HBox([valLow,valHigh])\n",
    "             ])\n",
    "display(slider_img)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Video_display():\n",
    "    global pan\n",
    "    global tilt\n",
    "    while True:   \n",
    "        frame = picamera.capture_array()\n",
    "        frame=cv2.flip(frame, 0)\n",
    "        hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)\n",
    "\n",
    "        hueLow=hueLower.value \n",
    "        hueUp=hueUpper.value\n",
    "\n",
    "        hue2Low=hue2Lower.value\n",
    "        hue2Up=hue2Upper.value\n",
    "\n",
    "        Ls= satLow.value\n",
    "        Us = satHigh.value\n",
    "\n",
    "        Lv=valLow.value\n",
    "        Uv=valHigh.value\n",
    "\n",
    "        l_b=np.array([hueLow,Ls,Lv])\n",
    "        u_b=np.array([hueUp,Us,Uv])\n",
    "\n",
    "        l_b2=np.array([hue2Low,Ls,Lv])\n",
    "        u_b2=np.array([hue2Up,Us,Uv])\n",
    "\n",
    "        FGmask=cv2.inRange(hsv,l_b,u_b)\n",
    "        FGmask2=cv2.inRange(hsv,l_b2,u_b2)\n",
    "        FGmaskComp=cv2.add(FGmask,FGmask2)\n",
    "        FGmaskComp_img.value = bgr8_to_jpeg(FGmaskComp)\n",
    "\n",
    "        contours,_=cv2.findContours(FGmaskComp,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)\n",
    "        contours=sorted(contours,key=lambda x:cv2.contourArea(x),reverse=True)\n",
    "\n",
    "        for cnt in contours:\n",
    "            area=cv2.contourArea(cnt)\n",
    "            (x,y,w,h)=cv2.boundingRect(cnt)\n",
    "            if area>=50:\n",
    "                cv2.rectangle(frame,(x,y),(x+w,y+h),(255,0,0),3)\n",
    "                objX=x+w/2\n",
    "                objY=y+h/2\n",
    "\n",
    "                errorPan=objX-width/2\n",
    "                errorTilt=objY-height/2            \n",
    "\n",
    "                if abs(errorPan)>15:\n",
    "                    pan=pan+errorPan/75    \n",
    "\n",
    "                if abs(errorTilt)>15:\n",
    "                    tilt=tilt+errorTilt/75\n",
    "\n",
    "                if pan>180:\n",
    "                    pan=180\n",
    "                    print(\"Pan Out of  Range\") \n",
    "                if pan<-90:\n",
    "                    pan=-90\n",
    "                    print(\"Pan Out of  Range\")              \n",
    "                if tilt>90:\n",
    "                    tilt=90\n",
    "                    print(\"Tilt Out of  Range\") \n",
    "                if tilt<-10:\n",
    "                    tilt=-10\n",
    "                    print(\"Tilt Out of  Range\")\n",
    "                    \n",
    "                clbrobot.set_servo_angle(10,pan)\n",
    "                clbrobot.set_servo_angle(9,tilt) \n",
    "                break \n",
    "                \n",
    "        frame_img.value = bgr8_to_jpeg(frame)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "t = threading.Thread(target=Video_display)\n",
    "t.setDaemon(True)\n",
    "t.start()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 如果摄像头云台方向不在正前方运行调整到正前方"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 舵机调零\n",
    "pan =  90\n",
    "tilt = -10\n",
    "# 频率设置为50hz，适用于舵机系统。\n",
    "clbrobot.set_servo_angle(10,pan)  # 底座舵机 90 \n",
    "clbrobot.set_servo_angle(9,tilt)  # 顶部舵机 30\n",
    "time.sleep(0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 结束线程\n",
    "stop_thread(t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
